#!/usr/bin/python
# -*- coding: UTF-8 -*-


import time,threading,sys
sys.path.append("/home/biowin/Minibot/04.RobotSamples/03.SpiderRobot")
from BwRobotLib import BwRobotLib
from SpiderRobot import SpiderRobot
from BwSensorLib import BwSensorLib
import os,inspect

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"

class DefenseRobot:
	def __init__(self):
		# 实例化传感器库对象
		self.sensorlib = BwsensorliblibLib("udp")
		# 实例化BwRobotLib库
		self.robotlib = BwRobotLib("udp")
		# 实例化蜘蛛机器人库
		self.spider = SpiderRobot(self.robotlib)
		# 连接机器人设备
		self.robotlib.connectRobot("10.10.100.254",8899) #真机AP模式
        # 导入机器人配置参数
        self.robotlib.loadConfig(config_file)
		#配置ID
		self.spider.bindIds(self.robotlib.module_ids)
		# 时间变量
		self.wait_time = 0.2
		# 点阵显示
		# 点阵图案：R G B　无表情　
		self.display_data = [[0x00,0x00,0xFC,0x14,0x22],[0x00,0x14,0x08,0x14,0x00],[0x22,0x14,0x08,0x14,0x22],
							[0x00,0x00,0x00,0x00,0x00]]	
        # RGB彩灯参数
        # [255,1,1] 红色；[1,255,1] 绿色；[1,1,255] 蓝色；[1,1,1] 灭灯;
        self.rgb_value = [[255, 1, 1], [1, 255, 1], [1, 1, 255], [1,1,1]]

    # 防御机器人： 识别不同颜色方块，做不同的防御动作，点阵显示不同图案，rgb灯亮不同的颜色
	def defenseRobotMotion(self):
		while 1:
			color = self.sensorlib.readColorSensor() # 读取颜色传感器数据
			time.sleep(self.wait_time)
			print ("返回值：",color)
			# 红色防御状态
			if(color==1): 
				self.sensorib.setMatrixSensor(self.display_data[0]) # 点阵显示字母‘R’
				self.sensorib.setRGBLightSensor(self.rgb_value[0]) # rgb灯亮红灯
				self.spider.doBattle() # 战斗状态
				self.spider.quickInit()	# 恢复初始动作	
			# 蓝色防御状态	
			elif(color==2):	
				self.sensorib.setMatrixSensor(self.display_data[1]) # 点阵显示字母‘G’
				self.sensorib.setRGBLightSensor(self.rgb_value[1]) # rgb灯亮绿灯
				self.spider.doMoveBody() # 扭动身体状态
				self.spider.quickInit() # 恢复初始动作	
			# 绿色防御状态
			elif(color==3):
				self.sensorib.setMatrixSensor(self.display_data[2]) # 点阵显示字母‘B’
				self.sensorib.setRGBLightSensor(self.rgb_value[2]) # rgb灯亮蓝灯
				self.spiderib.doHangTail() # 摇尾巴状态
				self.spiderib.quickInit() # 恢复初始动作	
			# 没有检测到颜色
			else:
				self.sensor.setMatrixSensor(self.display_data[3]) # 点阵不显示图案
				self.sensor.setRGBLightSensor(self.rgb_value[3]) # rgb灯熄灭
				self.spiderib.quickInit() # 恢复初始动作	

			

# 主函数			
def main():
    # 实例化防御机器人类
    defenseRobot = DefenseRobot()
    # 执行防御机器人功能
    defenseRobot.defenseRobotMotion()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass